/*
 * @Author: LVGRAPE
 * @LastEditors: LVGRAPE
 */
#ifndef __ESTIMATOR_H__
#define __ESTIMATOR_H__

#include "fc_sensors.h"
#include "zino.h"

#define ESTIMATOR_SAVED_FLAG 0XBF

#ifdef __cplusplus
extern "C"
{
#endif

    typedef struct
    {
        float pos[3];
        float vel[3];
        float acc[3];
    } estimator_t;

    struct emstimator_params
    {
        uint8_t saved_flag;
        uint8_t w_baro_p;
        uint8_t w_baro_v;
        uint8_t w_baro_a;
        uint8_t w_tof_p;
        uint8_t w_tof_v;
        uint8_t w_tof_a;
        uint8_t w_opt_p;
        uint8_t w_opt_v;
        uint8_t w_opt_a;
    };
    struct emstimator_params_f
    {
        float w_baro_p;
        float w_baro_v;
        float w_baro_a;
        float w_tof_p;
        float w_tof_v;
        float w_tof_a;
        float w_opt_p;
        float w_opt_v;
        float w_opt_a;
    };
    extern estimator_t fcEstimator;
    void estimator_update(float dt);
    void estimator_init(void);
    void reload_estimator_params(void);
    void save_estimator_params(void);
    void estimator_reset_params(void);
    float get_tof_alt(void);
    ZINO_CONFIG_DECLARE(struct emstimator_params, estimator_params_config);
#ifdef __cplusplus
}
#endif

#endif // !__ESTIMATOR_H__
